#pragma once
#include <Eigen/Core>
#include <map>
#include <memory>
#include <string>
#include <vector>

class octree;

struct las_Point {
  las_Point()
      : point_x(0.0f),
        point_y(0.0f),
        point_z(0.0f),
        color_r(0),
        color_b(0),
        color_g(0) {}
  las_Point(float x, float y, float z, int r, int g, int b)
      : point_x(x),
        point_y(y),
        point_z(z),
        color_r(r),
        color_g(g),
        color_b(b) {}
  las_Point(float x, float y, float z)
      : point_x(x),
        point_y(y),
        point_z(z),
        color_r(0),
        color_b(0),
        color_g(0) {}

  float point_x;
  float point_y;
  float point_z;
  uint8_t color_r;
  uint8_t color_g;
  uint8_t color_b;
};

class octreeNode {
  friend class octree;

 public:
  octreeNode();
  virtual ~octreeNode();
  virtual bool IsLeaf() const = 0;
  virtual void AddRecurPoint(const las_Point& las_pt, double resolution) = 0;
  virtual void Archive(std::ostream& ofs) = 0;
  virtual void Archive(std::istream& ifs) = 0;
  virtual void SetExtent(const Eigen::Vector3f& center, double radius);
  virtual void GetExtent(Eigen::Vector3f& min, Eigen::Vector3f& max);
  virtual float GetRadius();
  virtual const Eigen::Vector3f& GetCenter();

 protected:
  Eigen::Vector3f center_;
  float radius_;
};

class LeafNode : public octreeNode {
  friend class octree;

 public:
  virtual void AddRecurPoint(const las_Point& las_pt,
                             double resolution) override;
  virtual bool IsLeaf() const override;
  virtual void Archive(std::ostream& ofs) override;
  virtual void Archive(std::istream& ifs) override;
  const std::vector<las_Point>& GetPoints() const;
  void CyberSimplify(int cyber_num);

 private:
  std::vector<las_Point> points_;
};

class BranchNode : public octreeNode {
  friend class octree;

 public:
  BranchNode();
  virtual ~BranchNode();
  virtual void AddRecurPoint(const las_Point& las_pt,
                             double resolution) override;
  virtual bool IsLeaf() const override;
  void GetChildExtent(int idx, Eigen::Vector3f& center, double& radius);
  int GetChildIdx(float x, float y, float z);
  virtual void Archive(std::ostream& ofs) override;
  virtual void Archive(std::istream& ifs) override;

 protected:
  octreeNode* CreateOrGetChild(int idx, double resolution);

 private:
  octreeNode* children_[8];
};

class octree {
 public:
  octree();
  virtual ~octree();
  bool WriteOctreeFile(const std::string& filename);
  bool ReadOctreeFile(const std::string& filename);
  bool ReadPlyFile(const std::string& filename);
  void AddPoint(const las_Point& las_pt);
  std::vector<las_Point> GetPoints() const;
  void SetResolution(const double& resolution);
  double GetResolution() const;
  void SetCyberNum(int cyber_num);
  int GetCyberNum() const;
  std::vector<octreeNode*> GetLeaves() const;
  Eigen::Vector3d GetOffset() const;
  void Simplify(int min_points_leave = 5);
  bool IntersectWithLine(const Eigen::Vector3d& start_pt,
                         const Eigen::Vector3d& end_pt,
                         Eigen::Vector3d& ptHit) const;

 private:
  std::map<std::tuple<int, int, int>, std::shared_ptr<BranchNode>> roots_;
  Eigen::Vector3d offset_;
  Eigen::Vector3d min_;
  Eigen::Vector3d max_;
  float block_size_;
  float resolution_;
  int cyber_num_;
};
